#include <Servo.h>

int motor1_controlPin1 = 2;
int motor1_controlPin2 = 3;
int motor1_enabledPin = 5;
int servo_pin = 11;

int sensor1_pin = 7;

int sensor_1=0;
int audio_pin = 8;
int light_pin = A0;




Servo servo;

int m1_direction = 0;
int motor1_speed = 0;
int servo_angle = 90;
int light = 0;

int beep_hz=0;
int beep_msec=0;




void setup()
{
  Serial.begin(9600);
  
  /** SENSOR 1 **/
   pinMode(sensor1_pin,INPUT);  
  
/** MOTOR 1 **/
  pinMode(motor1_controlPin1,OUTPUT);
  pinMode(motor1_controlPin2,OUTPUT);  
  pinMode(motor1_enabledPin,OUTPUT);  
  
  motor1_stop();
  motor1_anticlockwise();
  
  
  /** SERVO **/
  servo.attach(servo_pin);
  
  /** AUDIO **/
  pinMode(audio_pin,OUTPUT);
  
  motor1_direction(m1_direction);
  motor1_setVel(motor1_speed);
  motor1_start();
  servo_turn(servo_angle);
  
}

void beep(int Hz,int msec)
{
  tone(audio_pin,Hz);//349Hz 
  delay(msec);
  noTone(audio_pin);
}

void motor1_stop()
{
  analogWrite(motor1_enabledPin, 0);
}

void motor1_start()
{
  analogWrite(motor1_enabledPin, motor1_speed);
}

void motor1_setVel(int v)
{
  motor1_speed=v;
 // analogWrite(motor1_enabledPin, motor1_speed);
}

void motor1_direction(int d)
{
  if(d==0)
  {
    motor1_clockwise();
  }
  else
  {
    motor1_anticlockwise();
  }
}

void motor1_clockwise()
{
  digitalWrite(motor1_controlPin2,LOW);
  digitalWrite(motor1_controlPin1,HIGH);
}

void motor1_anticlockwise()
{
  digitalWrite(motor1_controlPin2,HIGH);
  digitalWrite(motor1_controlPin1,LOW);
}

void servo_turn(int angle)
{
//  if(abs(servo.read()-angle)>1)
//  {
  servo_angle = angle;//%180;
  servo.write(servo_angle);
//  }
  
}


void serialEvent()
{
   int n = Serial.available();
  if(n==5)
  {

  
       servo_angle = (int) (Serial.read());
       m1_direction=(int)(Serial.read());
       motor1_speed=(int)(Serial.read());
       beep_hz=(int)(Serial.read());
       beep_msec=(int)(Serial.read());
       
       //  Serial.println("servo_angle"+servo_angle);
       servo_turn(servo_angle);
       motor1_direction(m1_direction);
       motor1_setVel(motor1_speed);
       motor1_start();
        
        if(beep_msec>0)
        {
          beep(beep_hz,beep_msec);
        }
  
  }

  

}




void loop()
{
 
  

 
  light = analogRead(light_pin);
  sensor_1=digitalRead(sensor1_pin);
//      Serial.println(servo_angle);
    Serial.print("0;0;0;0;0;");
    Serial.print(motor1_speed);
    Serial.print(';');
    Serial.print(servo.read());
    Serial.print(';');   
    Serial.print(sensor_1);
    Serial.print(';');
    Serial.print(light);
    Serial.println(';');
    Serial.flush();


  


 



 delay(1);
 //Serial.println(servo.read());
  // Serial.write("servo_angle");
//Serial.write(servo.read());
//

 
}
